#include <iostream>
#include "SMATModel.hh"
#include "display.hh"
#include "utils.hh"
#include <string.h>

#include <pthread.h>
#include <stdlib.h>
#include <stdio.h>
#include <semaphore.h>

#include <sys/time.h>
#include <unistd.h>
#include <sys/select.h>
#include "smat-ransac.h"


#include "videoAVI.h"
#include "constants.h"
#include "face_detect_vj.hh"
#include "modelo_RANSAC.h"
#include "read_video.h"
#include "other_functions.h"
#include "options.h"
#include <colores.h>

#define SHOW_LEFT
//#define SHOW_RIGHT 
//#define USE_THREAD_RANSAC

int bucle_cvWaitKey(int * wait);


int main(int argc, char *argv[])
{
    //entorno grafico  
    char Model_SMAT[2][100] = { "Model+SMAT", "Model+SMAT Der"};
    boost::shared_ptr<CvVideoWriter> video_results_SMAT, video_results_FaceDir;
    int i;
    boost::shared_ptr<CvFileStorage> pose_results;
    boost::shared_ptr<IplImage> srcD, srcI;
    CALIBRATION_PARAMS_t calibration_paramsL, calibration_paramsR;
    CvRect haar_modelo = {223, 175, 341, 455};

    CvMat *points3D, *pointsID[2];
    CvMat *angY;
    CvMat * chesscorners;
    IplImage *image2[2];
    CvMat * lines;
    CvMat *epilines;
    CvMat *rot;
    CvMat * mask_show;
    int npoints;

    //srand( (unsigned)time( NULL ) );
    srand(1);  //siempre la misma secuencia aleatoria. Debug

    if (!leer_opciones(argc, argv))
	return 0;

    int graficar = ini_opt("graficar");
    /***********/
    //lanza el hilo de lectura de las imágenes
    video_params_t video_params;
    int iniframe;
    stereo_IplImage_t *imgs;
    int wait = ini_opt("wait");

    IplImage *clonek, *display;
    IplImage * buffer_show_imgs[SHOW_IMAGE_BUFFER_SIZE];
    IplImage * buffer_display_smat[SHOW_IMAGE_BUFFER_SIZE];
    int np_to_show_imgs=0, np_to_show_imgs2;

    cvNamedWindow("SMAT", 1);
    cvMoveWindow("SMAT", 0,1000);
    cvNamedWindow(window[0], 1);
    cvNamedWindow(window[1], 1);
    cvMoveWindow(window[0],1000,0);
    cvMoveWindow(window[1],0,1000);

    int lastframe=0;

    iniframe = ini_opt("iniframe");

    //lanza el hilo de lectura de las imágenes
    iniframe = start_ReadVideo_thread(&video_params, ini_opt("cam"), 
				      ini_opts("v"), ini_opts("format"), ini_opt("iniframe"));

    //búsqueda del fichero de calibración y 
    //  leer los parámetros de calibración
    char buffer[400];
    int num_video;
    FILE *fcal;
    char * index1, *index2;
    strcpy (buffer, video_params.videoI_name);
    index1 = rindex(buffer, '/');
    if (!index1)
	index1 = video_params.videoI_name;
    index1++;
    index2 = index(index1, '_');
    if (!index2)
    {
	printf("El nombre del fichero");
    }
    index2++;
    sscanf(index2, "%d", &num_video);
    sprintf(index2, "%d_1.cal", num_video);
    fcal = fopen(buffer, "r");
    while (fcal == NULL && num_video)
    {
	num_video--;
	sprintf(index2, "%d_1.cal", num_video);
	fcal = fopen(buffer, "r");
    }
    if (fcal == NULL)
    {
	printf("Error al buscar el fichero de calibración.\n");
	exit(-1);
    }
    fclose(fcal);

    printf("fichero de\ncalibración: %s\n", buffer);

    CvFileStorage* fs = cvOpenFileStorage( buffer, 0, CV_STORAGE_READ );
    calibration_paramsL.Fundamental = (CvMat *) cvReadByName(fs, NULL, "Fundamental");
    calibration_paramsL.R = (CvMat *) cvReadByName(fs, NULL, "R");
    calibration_paramsL.T = (CvMat *) cvReadByName(fs, NULL, "T");
    calibration_paramsL.KK = (CvMat *) cvReadByName(fs, NULL, "Fl");
    calibration_paramsR.KK = (CvMat *) cvReadByName(fs, NULL, "Fr");
    
    struct corners_params_t cp;
    cp.block_size = cvReadInt(cvGetFileNodeByName( fs, NULL, "block_size"), 19) ;
    cp.k_harris = cvReadReal(cvGetFileNodeByName( fs, NULL, "k_harris"), 0.2375);
    cp.min_distance = cvReadInt(cvGetFileNodeByName( fs, NULL, "min_distance"), 30);
    cp.quality_level_max=cvReadReal(cvGetFileNodeByName(fs, NULL, "quality_level_max"),0.006);
    cp.quality_level_min=cvReadReal(cvGetFileNodeByName(fs, NULL, "quality_level_min"),0.0000400);
    cvReleaseFileStorage( &fs );

    boost::shared_ptr<SMATModel> smatI;
    boost::shared_ptr<SMATModel> smatD; 

    printf("\nModelo:%s.\n\n", ini_opts("m"));
    fflush(stdout);

    FILE * fout = NULL;
    int save_video = ini_opt("ov");
    int save_pose = ini_opt("op");
    FILE * videofile =0;
    struct timeval oldtimeleft={0,0}, oldtimeright={0,0};

    if (iniframe < 0)
    {
	printf("error en la lectura de los videos.\n");
	fflush(stdout);
	exit(-1);
    }
    if (!get_read_images(&imgs, &video_params, false))
    {
	printf("Error while reading images\n"); fflush(stdout);
	exit(-1);
    }

    //  leer el modelo de un archivo
    //inicializar cosas de Pedro
    CvMat *vectL, *vect_media_L, *translationL, *translation_media_L, *vectRl, *translationRl;
    CvMat *vectR, *vect_media_R, *translationR, *translation_media_R;
    CvMat *rotation;
    CvMat * offsettrans, *offsettrans2;
    float *x2, *y2, *z2;

    vectL = cvCreateMat(3,1, CV_32FC1);
    vectR = cvCreateMat(3,1, CV_32FC1);
    vectRl = cvCreateMat(3,1, CV_32FC1);
    vect_media_L = cvCreateMat(3,1, CV_32FC1);
    vect_media_R = cvCreateMat(3,1, CV_32FC1);
    translation_media_L = cvCreateMat( 3, 1, CV_32FC1 );
    translation_media_R = cvCreateMat( 3, 1, CV_32FC1 );
    translationL = cvCreateMat( 3, 1, CV_32FC1 );
    translationR = cvCreateMat( 3, 1, CV_32FC1 );
    translationRl = cvCreateMat( 3, 1, CV_32FC1 );
    rotation = cvCreateMat( 3, 3, CV_32FC1 );

    if (ini_opt("r"))
    {
	strcpy(buffer, video_params.videoI_name);
	sprintf(buffer + strlen(buffer)-4, ".da%d", ini_opt("dat"));
	
	if (0 == (npoints = ReadModelFromFile(buffer, &pointsID[0], &pointsID[1], &points3D, &angY, &chesscorners)))
	    exit(-1);
    }
    //cvScale(points3D, points3D, 28.0);

    /*
    //calculo de las normas
    CvMat r1, r2;
    int j;
    double dist;
    for (j=0; j<points3D->rows; j++)
    for (i=1; i<points3D->rows; i++)
    {
    cvGetRow(points3D, &r1, j);
    cvGetRow(points3D, &r2, i);
    dist = cvNorm(&r1, &r2);
    printf("Norma %d - %d: %6.3lf\n", j,i,dist-(int)(dist/28)*28);
    } 
    */
    // si no se inidica el frame inicial, o este es el frame 0, 
    //hace un repaso para elegir el frame inicial
    if (1)//iniframe == 0)
    {
	do 
	{
	    for (i=0; i<npoints; i++)
	    {
		cvCircle (imgs->left, cvPoint((int)cvmGet(pointsID[0], i, 0), (int)cvmGet(pointsID[0], i, 1)), 7,  CV_RGB(0,0,0), 1, CV_AA, 0);
		cvCircle (imgs->right, cvPoint((int)cvmGet(pointsID[1], i, 0), (int)cvmGet(pointsID[1], i, 1)), 7,  CV_RGB(0,0,0), 1, CV_AA, 0);
	    }
	    cvShowImage(window[0], imgs->left);
	    cvShowImage(window[1], imgs->right);

	    if (!get_read_images(&imgs, &video_params, true))
	    {
		printf("Error while reading images\n"); fflush(stdout);
		exit(-1);
	    }
	} while (bucle_cvWaitKey(&wait)>=0);
    }

    if(save_video)
    {
	std::string fs_basename = get_basename(video_params.videoI_name);

	char file[fs_basename.size()+50];

	sprintf(file, "%s_results_FaceDir.avi", fs_basename.c_str());
	printf("save video en: %s\n", file);
	fout = fopen ("data.txt", "w");
	videofile = fopen("video.raw", "w");
    }

    if(save_pose)
    {
	std::string fs_basename = get_basename(video_params.videoI_name);

	char file[fs_basename.size()+50];

	sprintf(file, "%s_pose_results.xml", fs_basename.c_str());
	pose_results.reset(cvOpenFileStorage(file, NULL, CV_STORAGE_WRITE), deleteFileStorage);		
    }

    std::vector<boost::shared_ptr<CvMat> > smat_pts;
    IplImage *img[2], *img_smat[2];

    CvSize sori = cvGetSize(imgs->left);
    CvSize sdest = sori; //{800, 800};

    img[0] = cvCreateImage(sdest, IPL_DEPTH_8U, 1);
    img[1] = cvCreateImage(sdest, IPL_DEPTH_8U, 1);

    cvCopy(imgs->left, img[0]);// = imgs->left;
    cvCopy(imgs->right, img[1]);// = imgs->right;

    sdest = cvSize(800, 800);
    img_smat[0] = cvCreateImage(sdest, IPL_DEPTH_8U, 1);
    img_smat[1] = cvCreateImage(sdest, IPL_DEPTH_8U, 1);

    CvRect roiori = {(sori.width-sdest.width)/2, 0, sdest.width, sori.height};
    CvRect roidest = {0,(sdest.height-sori.height)/2, sdest.width, sori.height};
    
    cvZero(img_smat[0]);
    cvZero(img_smat[1]);
    cvSetImageROI(imgs->left, roiori);
    cvSetImageROI(imgs->right, roiori);
    cvSetImageROI(img_smat[0], roidest);
    cvSetImageROI(img_smat[1], roidest);
    
    cvCopy(imgs->left, img_smat[0]);// = imgs->left;
    cvCopy(imgs->right, img_smat[1]);// = imgs->right;
    
    cvResetImageROI(imgs->left);
    cvResetImageROI(imgs->right);
    cvResetImageROI(img_smat[0]);
    cvResetImageROI(img_smat[1]);

    InitModelo(&calibration_paramsL, &calibration_paramsR);
    cvShowImage(window[0], img_smat[0]);
    cvShowImage(window[1], img_smat[1]);

    printf("primer frame\n");
    cvWaitKey(10);


    /***********************************************************************************************
     ** comienzo del algoritmo
     ***********************************************************/
    
    // leer el modelo de un archivo o crearlo
    if (!ini_opt("r"))
    {
	npoints = do_Estereo(img, 35,  
			     &pointsID[0], &pointsID[1], &points3D, &angY,
			     &calibration_paramsL, &calibration_paramsR, &cp,
			     haar_modelo, 
			     graficar);
    } 
    printf("Tamaño de la imagen: %d x %d\n", cvGetSize(img[0]).width, cvGetSize(img[0]).height);

    lines = cvCreateMat (npoints, ini_opt("lines"), CV_32FC1);
    epilines = cvCreateMat (npoints, 3, CV_32FC1);
    rot = cvCreateMat(3,3, CV_32FC1);
    mask_show = cvCreateMat(npoints, 1, CV_8UC1);

    int index_punto =0;
    struct mousecalldata_t paramsMouseCall[2];

    cvSet(mask_show, cvScalar(1));
    //cvSetReal1D(mask_show, 0, 0);

    FindCornerSubPix(imgs->left, pointsID[0], chesscorners);
    FindCornerSubPix(imgs->right, pointsID[1], chesscorners);

    //GetStereoPoint(pointsID[0], pointsID[1], &calibration_paramsL, &calibration_paramsR, points3D);
    cvComputeCorrespondEpilines(pointsID[0], 1, calibration_paramsR.Fundamental, epilines);
    LineasModelo(points3D, lines);

    CvScalar offsetmodel = CenterModel(points3D);    
    
    Modelo(points3D, pointsID[0], mask_show, calibration_paramsL.KK, rotation, translationL, vectL);
    
    ModeloEx(points3D, pointsID[0], chesscorners, calibration_paramsL.KK, rotation, translationL, vectL);

    cvmSet(translationL, 0,0,0);
    cvmSet(translationL, 1,0,0);

    for (i=0; i<2; i++)
    {
	image2[i] = cvCreateImage(cvGetSize(img[i]), IPL_DEPTH_8U, 3);
	cvConvertImage(img[i], image2[i]);

	paramsMouseCall[i].points = pointsID[i];
	paramsMouseCall[i].pointsI = pointsID[0];
	paramsMouseCall[i].pointsD = pointsID[1];
	paramsMouseCall[i].points3D = points3D;
	paramsMouseCall[i].epilines = epilines;
	paramsMouseCall[i].vect = vectL;
	paramsMouseCall[i].translation = translationL;
	paramsMouseCall[i].lines = lines;
	paramsMouseCall[i].calL = &calibration_paramsL;
	paramsMouseCall[i].calR = &calibration_paramsR;
	paramsMouseCall[i].image = img;
	paramsMouseCall[i].ID = i;
	paramsMouseCall[i].index = &index_punto;
	paramsMouseCall[i].angY = angY;
	paramsMouseCall[i].mask_show = mask_show;
	paramsMouseCall[i].offsetmodel = offsetmodel;
	paramsMouseCall[i].chesscorners = chesscorners;
	//cvResizeWindow(window[i], 600,600);

	cvSetMouseCallback(window[i] ,  on_mouse,  (void*)&paramsMouseCall[i]);    
    }
    
    DibujarStereo(image2, 2, pointsID[0], pointsID[1], chesscorners, NULL);
    for (i=0; i<2; i++)
    {
	cvShowImage(window[i], image2[i]);
	cvReleaseImage(&image2[i]);
    }
    
    cvSet(mask_show, cvScalar(1));
    MostrarModelo(&paramsMouseCall[0]);
    
    npoints = points3D->rows;

    FindCornerSubPix(imgs->left, pointsID[0], chesscorners);
    FindCornerSubPix(imgs->right, pointsID[1], chesscorners);

    printf("cvFindCornerSubPix\n");
    //GetStereoPoint(pointsID[0], pointsID[1], &calibration_paramsL, &calibration_paramsR, points3D);
    LineasModelo(points3D, lines);

    //centra el modelo sobre el punto 0 para un correcto uso de cvPOSIT / cvProjectPoints2
    offsetmodel = CenterModel(points3D);    
    
    //creación del modelo smat
    smatD= load_SMAT_Model(ini_opts("m"), npoints);
    smatI= load_SMAT_Model(ini_opts("m"), npoints);
    print_model_info(smatI);

    cvDestroyWindow(window[1]);

    //mostrar modelos iniciales sobre las imágenes
    if (1)
    {
	cvSet(mask_show, cvScalar(1));
	cvSetReal1D(mask_show, 0, 0);
	ModeloEx(points3D, pointsID[0], chesscorners, calibration_paramsL.KK, rotation, translationL, vectL);

	offsettrans = cvCloneMat(translationL);
	offsettrans2 = cvCloneMat(translationL);
	x2 = &offsettrans->data.fl[0];
	y2 = &offsettrans->data.fl[1];
	z2 = &offsettrans->data.fl[2];
	printf ("offsettrans: frame %d -> (%f %f %f)\n", lastframe, *x2, *y2, *z2);

	cvNamedWindow("Modelo3D Der", 0);
	cvResizeWindow("Modelo3D Der", 600,600);
	cvNamedWindow("Modelo3D", 0);
	cvResizeWindow("Modelo3D", 600,600);

	IplImage *clonekI = cvCreateImage(cvGetSize(img[0]), IPL_DEPTH_8U, 3);
	IplImage *clonekD = cvCreateImage(cvGetSize(img[1]), IPL_DEPTH_8U, 3);
	cvSet(mask_show, cvScalar(1));

	cvConvertImage(img[0], clonekI);
	DibujarModelo3D(clonekI, points3D,  &calibration_paramsL, translationL, vectL,  lines, mask_show, NULL, CV_RGB(255,0,0));
	cvShowImage("Modelo3D", clonekI);
	// obtenemos vectR y translationR en coordenadas de la cámara derecha
	ChangeView(vectL, translationL, &calibration_paramsR, vectR, translationR, rot);
	//Modelo(points3D, pointsID[1], NULL, calibration_paramsR.KK, rotation, translationR, vectR);
	cvConvertImage(img[1], clonekD);
	DibujarModelo3D(clonekD, points3D,  &calibration_paramsR, translationR, vectR,  lines, mask_show);
	cvShowImage("Modelo3D Der", clonekD);

	cvConvertImage(img[0], clonekI);
	DibujarModelo(clonekI, pointsID[0], &calibration_paramsL, translationL, vectL,  lines, mask_show);
	cvShowImage(Model_SMAT[0], clonekI);

	/*
	  printf("------------------------------------------------------\n");
	  cvWaitKey(0);
	  //ahora tenemos el modelo 3d exacto y bien colocado
	  cvRodrigues2(vectL, rotation);
	  cvGEMM(points3D, rotation, 1, NULL, 1, points3D, CV_GEMM_B_T + CV_GEMM_C_T);
	  cvSubS(angY, cvScalar(cvmGet(vectL,1,0)), angY);
	  CvMat mat;
	  for (i=0; i<3; i++)
	  {
	  cvGetCol(points3D, &mat, i);
	  cvAddS(&mat, cvScalar(cvmGet(translationL, i, 0)), &mat);
	  }
	  ModeloEx(points3D, pointsID[0], chesscorners, calibration_paramsL.KK, rotation, translationL, vectL);

	
	CvMat *angY2;
	CvFileStorage* fs;
	fs = cvOpenFileStorage( "modelo3D.dat", 0, CV_STORAGE_WRITE );
	cvWrite(fs, "pointsI", pointsID[0]);
	cvWrite(fs, "pointsD", pointsID[1]);
	cvWrite(fs, "points3D",points3D);
	angY2 = cvCloneMat(angY);
	cvScale(angY2,angY2, 180.0F/CV_PI);
	cvSubS(angY2, cvScalar(4), angY2);
	cvWrite(fs, "angY", angY2);
	cvWrite(fs, "chesscorners", chesscorners);
	cvReleaseFileStorage(&fs);
	cvReleaseMat(&angY2);	
	*/
	cvConvertImage(img[0], clonekI);
	DibujarModelo(clonekI, pointsID[0], &calibration_paramsL, translationL, vectL,  lines, mask_show);
	DibujarModelo3D(clonekI, points3D,  &calibration_paramsL, translationL, vectL,  lines, mask_show, NULL, CV_RGB(255,0,0));
	cvShowImage("Modelo3D", clonekI);
	cvWaitKey(0);

	cvDestroyWindow("Modelo3D");
	cvDestroyWindow("Modelo3D Der");

	cvReleaseImage(&clonekI);
	cvReleaseImage(&clonekD);
    }
    

#ifdef SHOW_LEFT
    cvNamedWindow(Model_SMAT[0]);
    cvResizeWindow(Model_SMAT[0], 600,600);
    cvMoveWindow(Model_SMAT[0], 1000,0);
#endif
#ifdef SHOW_RIGHT
    cvNamedWindow(Model_SMAT[1], 1);
    cvResizeWindow(Model_SMAT[1], 600,600);
#endif

    
    ModeloEx(points3D, pointsID[0], chesscorners, calibration_paramsL.KK, rotation, translationL, vectL);

    // pasamos de vectI a vectR, a coordenadas de la cámara derecha
    ChangeView(vectL, translationL, &calibration_paramsR, vectR, translationR, rot);

    // desplazamos los puntos del modelo para que el SMAT use el punto en el centro, no en la esquina.
    CvMat pointsID2[2];
    CvScalar desplazar_parche = cvScalar( 15 + roiori.x , 15 - roidest.y, 0, 0);
    cvReshape(pointsID[0], &pointsID2[0], 2, 0);
    cvReshape(pointsID[1], &pointsID2[1], 2, 0);
    cvSubS(&pointsID2[0], desplazar_parche, &pointsID2[0]);
    cvSubS(&pointsID2[1], desplazar_parche, &pointsID2[1]);

    boost::shared_ptr<CvMat> mp (cvCreateMat(2, pointsID[0]->rows, pointsID[0]->type), deleteCvMat);
    smat_pts.push_back(mp);
    mp.reset(cvCreateMat(2, pointsID[0]->rows, pointsID[0]->type), deleteCvMat);
    smat_pts.push_back(mp);
    
    // REdesplazamos los puntos del modelo para que el SMAT use el punto en el centro, no en la esquina.
    cvAddS(&pointsID2[0], desplazar_parche, &pointsID2[0]);
    cvAddS(&pointsID2[1], desplazar_parche, &pointsID2[1]);

    CvMat *mask_showL = cvCreateMat(npoints, 1, CV_8UC1);	    	    
    CvMat *maskoutL = cvCreateMat(npoints, 1, CV_8UC1);	    	    
    CvMat *maskoutL2 = cvCreateMat(npoints, 2, CV_8UC1);
    CvMat *masktemp = cvCreateMat(npoints, 1, CV_8UC1);	    	    

    CvMat *points2D_reprojectI =  cvCreateMat(npoints, 2, CV_32FC1); 
    int inliersI;

    unsigned int frame = 0;
    bool quit = false;

    thread_params_t params_left;
    thread_params_t *p = &params_left;
    sem_t sem1, sem2;

    p->smat_pts = &smat_pts.at(0);
    p->smat = &smatI;
    p->src = &srcI;
    p->mask_show = mask_showL;
    p->maskout = maskoutL;
    p->pointsID = pointsID[0];
    p->inliers =0;
    p->points3D = points3D;
    p->cal = &calibration_paramsL;
    p->translation = translationL;
    p->vect = vectL;
    p->vect_media = vect_media_L;
    p->quit = 0;
    p->desplazar_parche = desplazar_parche;
    p->angY = angY;
    p->sem1 = &sem1;
    p->sem2 = &sem2;
    p->n = 0;
    p->img = img[0];
    p->img_smat = img_smat[0];
    p->chesscorners = chesscorners;

    //*************************************************************

    int key =0;
    wait = ini_opt("wait");
    cvDestroyWindow("Camara Izq");
    CvMat * offsetvect = 0;
    //vector inicial
    if (strlen(ini_opts("inivect")))
    {
	offsetvect = cvCloneMat(vectL);
	float ai, bi, gi;
	sscanf(ini_opts("inivect"), "%f,%f,%f", &ai, &bi, &gi);
	cvmSet(offsetvect, 0,0, ai/180*CV_PI);
	cvmSet(offsetvect, 1,0, bi/180*CV_PI);
	cvmSet(offsetvect, 2,0, gi/180*CV_PI);
    }

    //inicializaci el buffer de SHOW_IMAGE_BUFFER_SIZE imagenes para poder ir hacia atras
    np_to_show_imgs = 0;
    for (i=0; i<SHOW_IMAGE_BUFFER_SIZE; i++)
    {
	buffer_show_imgs[i] = cvCreateImage(cvGetSize(p->img), IPL_DEPTH_8U, 3);
	buffer_display_smat[i] = cvCreateImage(DISPLAY_SIZE, IPL_DEPTH_32F, 3);
    }

    CvMat * pointsID_old[2];
    pointsID_old[0] = cvCloneMat(pointsID[0]);
    pointsID_old[1] = cvCloneMat(pointsID[1]);

    int modificando=0, modificado=0;
    float incremento=0.5/180.0*CV_PI;
    float incrementopos = 2;
    float *a, *b, *c, *x, *y, *z;
    float *a2, *b2, *c2;
    //float basura;
    CvMat * vect_2 = cvCloneMat(vectL);

    a = &vect_media_L->data.fl[0];
    b = &vect_media_L->data.fl[1];
    c = &vect_media_L->data.fl[2];

    x = &translation_media_L->data.fl[0];
    y = &translation_media_L->data.fl[1];
    z = &translation_media_L->data.fl[2];

    a2 = &vect_2->data.fl[0];
    b2 = &vect_2->data.fl[1];
    c2 = &vect_2->data.fl[2];

    int frame2;
    //cvZero(offsettrans);
    CvMat * oldtrans = cvCloneMat(translation_media_L);
    double disttrans;
    cvZero(vectL);
    cvCopy(vectL, vect_media_L);

    while(!quit)
    {
	stereo_IplImage_t *imgs;
	if (!get_read_images(&imgs, &video_params, true))
	    break;
	
	if (imgs->hleft->nframe == 312)
	    printf("frame 312\n");

	frame2 = imgs->hleft->nframe;
	cvCopy(imgs->left, img[0]);// = imgs->left;
	cvCopy(imgs->right, img[1]);// = imgs->right;

	cvCopy (pointsID[0], pointsID_old[0]);
	cvCopy (pointsID[1], pointsID_old[1]);
	
	cvZero(img_smat[0]);
	cvZero(img_smat[1]);
	cvSetImageROI(imgs->left, roiori);
	cvSetImageROI(imgs->right, roiori);
	cvSetImageROI(img_smat[0], roidest);
	cvSetImageROI(img_smat[1], roidest);
        
	cvCopy(imgs->left, img_smat[0]);// = imgs->left;
	cvCopy(imgs->right, img_smat[1]);// = imgs->right;

	cvResetImageROI(imgs->left);
	cvResetImageROI(imgs->right);
	cvResetImageROI(img_smat[0]);
	cvResetImageROI(img_smat[1]);
	
	oldtimeright = imgs->hright->filltime;
	oldtimeleft = imgs->hleft->filltime;

	/**************  ALGORITMO ***************************************************/

	cvCopy(translation_media_L, oldtrans);
	SMAT_RANSAC2(&params_left);
		    
	//disttrans = cvNorm(oldtrans, translationL, CV_L2);
	if (0)//disttrans > 35 || disttrans < -35)
	{
	    printf("-- disttrans %3.2f\n", disttrans);
	    smatI->remove_last_added(p->maskout);
	    SMAT_RANSAC(&params_left);
	}
	inliersI = p->inliers;
		   
	np_to_show_imgs++;
	np_to_show_imgs %= SHOW_IMAGE_BUFFER_SIZE;
	clonek = buffer_show_imgs[np_to_show_imgs];
	display = buffer_display_smat[np_to_show_imgs];

	display_SMATModel(display, srcI.get(), smatI);

	if (1)
	{ 
	    cvProjectPoints2(p->points3D, vectL, translationL, calibration_paramsL.KK, NULL, points2D_reprojectI);
	    cvNot(p->maskout, masktemp);//invert the mask
	    cvAnd(masktemp, mask_showL, masktemp);
	    smatI->remove_last_added(masktemp);
	    //update the value of the outlier points with the reprojected value
	    cvRepeat(masktemp, maskoutL2);
	    cvCopy(points2D_reprojectI, pointsID[0]);//, maskoutL2);
	}

	//busca el punto exacto con el findsubpixel
	FindCornerSubPix(p->img, p->pointsID, p->mask_show);
	ModeloEx(p->points3D, p->pointsID, p->mask_show, p->cal->KK, rotation, p->translation, p->vect);
	cvCopy(vectL, vect_media_L);
	cvCopy(translationL, translation_media_L);

	do
	{
	    printf(CRED "inliersI : %d, error: %f" CNORMAL "\n", inliersI, p->error);
	    CvMat *vect2 = cvCreateMat(3, 1, CV_32FC1);
	    cvScale(vectL, vect2, 180.0/CV_PI);
	    printf("vectL: ");
	    print_arr(vect2, true, "%1.2f,");
	    printf("translationL: ");
	    print_arr(translationL, true, "%3f, ");
	    print_arr(p->maskout, 1, "%3.0f, ");
	    print_arr(masktemp, 1, "%3.0f, ");
	    cvReleaseMat(&vect2);

	    cvConvertImage(p->img, clonek);
	    DibujarModelo(clonek, pointsID[0], &calibration_paramsL, translation_media_L, 
			  vect_media_L,lines, mask_showL, p->maskout, CV_RGB(00,60,200), -1, offsetvect);
	    DibujarModelo3D(clonek, p->points3D,  &calibration_paramsL, translation_media_L, 
			    vect_media_L,
			    lines, mask_showL,  p->maskout, CV_RGB(255,0,0), -1, false, offsetvect);
	    cvShowImage(Model_SMAT[0], clonek);
	    cvShowImage("SMAT", display);
	
	    np_to_show_imgs2 = np_to_show_imgs;

	    modificando = 0;
	    while ((key = bucle_cvWaitKey(&wait)) > 0)
	    {
		if (!(key & 0xFF0000))
		{
		    switch (key)
		    {
		    case 'I':
		    case 'i':
			print_model_state(smatD);	    
			break;
		    case  0xFF51: //BACK ARROW
			//ir hacia atras en las imagenes a mostrar
			np_to_show_imgs2 = (np_to_show_imgs2-1+SHOW_IMAGE_BUFFER_SIZE) 
			    % SHOW_IMAGE_BUFFER_SIZE;
			printf("image frame diff: %d / %d\n", imgs->hleft->nframe +(
										    (np_to_show_imgs2 - np_to_show_imgs - SHOW_IMAGE_BUFFER_SIZE )
										    % SHOW_IMAGE_BUFFER_SIZE), imgs->hleft->nframe);
			cvShowImage(Model_SMAT[0], buffer_show_imgs[np_to_show_imgs2]);
			cvShowImage("SMAT", buffer_display_smat[np_to_show_imgs2]);
			break;
		    case  0xFF53: //NEXT ARROW
			//ir hacia atras en las imagenes a mostrar
			np_to_show_imgs2 = (np_to_show_imgs2+1) % SHOW_IMAGE_BUFFER_SIZE;
			printf("image frame diff: %d / %d\n", imgs->hleft->nframe +(
										    (np_to_show_imgs2 - np_to_show_imgs - SHOW_IMAGE_BUFFER_SIZE )
										    % SHOW_IMAGE_BUFFER_SIZE), imgs->hleft->nframe);
			cvShowImage(Model_SMAT[0], buffer_show_imgs[np_to_show_imgs2]);
			cvShowImage("SMAT", buffer_display_smat[np_to_show_imgs2]);
			break;

		    case 0xFF52: //UP
			break;
		    case 0xFF54: //DOWN
			break;
		    default:
			printf("Undefined key %d (%X)\n", key, key);
		    }
		}
		else
		{
		    switch(key)
		    {
			//SHIFT + ...
		    case  0x1FF51: //left
			*c += -incremento;
			break;
		    case  0x1FF53: //right
			*c += incremento;
			break;

			//SHIFT
		    case 0x1FF52: //UP
			*z2 += -incrementopos;
			*z += -incrementopos;
			break;
		    case 0x1FF54: //DOWN
			*z2 += incrementopos;
			*z += incrementopos;
			break;

			//ctrl + ...
		    case  0x4FF51: //left
			*b += incremento;
			break;
		    case  0x4FF53: //right
			*b += -incremento;
			break;
		    case 0x4FF52: //UP
			*a += -incremento;
			break;
		    case 0x4FF54: //DOWN
			*a += incremento;
			break;

			//ALT + ...
		    case  0x8FF51: //left
			*x2 += -incrementopos;
			*x += -incrementopos;
			break;
		    case  0x8FF53: //right
			*x2 += incrementopos;
			*x += incrementopos;
			break;
		    case 0x8FF52: //UP
			*y2 += -incrementopos;
			*y += -incrementopos;
			break;
		    case 0x8FF54: //DOWN
			*y2 += incrementopos;
			*y += incrementopos;
			break;
		    default:
			printf("Undefined key %d (%X)\n", key, key);
		    }
		    modificado = modificando = 1;
		    key = 1;
		    wait=0;
		    break;
		} //if (key & 0xFF0000)
	    }  //while ((key = bucle_cvWaitKey(&wait)) > 0)
	    if(key < 0)
	    {
		quit = true;
		p->quit = true;
	    }
	} while (modificando && key > 0);

	if (modificado)
	{
	    modificado =0;
	    printf("-- modificado --\n");
	    fflush(stdout);

	    cvProjectPoints2(p->points3D, vect_media_L, translation_media_L, 
			     calibration_paramsL.KK, NULL, points2D_reprojectI);
	    //update the value of the outlier points with the reprojected value
	    cvCopy(points2D_reprojectI, pointsID[0]);
	}

	if(save_video && frame > 0)
	{	    	    
	    printf("Saved video\n");
	    //write results
	    IplImage *final2 = cvCreateImage(cvGetSize(display), IPL_DEPTH_8U, 3);
	    cvConvertImage(display, final2);

	    int w = 280;
	    int h = (int)((float)final2->height / (float) final2->width * (float)w); 
	    cvSetImageROI(clonek, cvRect(0, cvGetSize(clonek).height-h, w, h));
	    if (0)
		cvResize(final2, clonek, CV_INTER_CUBIC);
	    //cvConvertImage(display, final2);
	    //cvSetImageROI(clonek, cvRect(0, 800-h, w, h));
	    //cvResize(final2, clonek, CV_INTER_CUBIC);
	    cvResetImageROI(clonek); 

	    cvReleaseImage(&final2);

	    //cvWriteFrame(video_results_FaceDir.get(), clonek);
	    if (videofile)
		fwrite(clonek->imageData, clonek->imageSize, 1, videofile);


	    fprintf(fout, "%d %f %f %f %f %f %f\n",
		    //imgs->hleft->nframe, 
		    frame2,
		    cvmGet(vect_media_L,0,0), cvmGet(vect_media_L,1,0),cvmGet(vect_media_L,2,0),
		    cvmGet(translation_media_L,0,0), cvmGet(translation_media_L,1,0),
		    cvmGet(translation_media_L,2,0));
	    fflush(fout);

	    printf("---------------- saving frame %d --------------\n", frame);
	}
	frame ++;
    }

    //libera los buffers que se crearon para poder rebobinar
    for (i=0; i<SHOW_IMAGE_BUFFER_SIZE; i++)
    {
	cvReleaseImage(&buffer_show_imgs[i]);
	cvReleaseImage(&buffer_display_smat[i]);
    }
    if (fout)
	fclose(fout);

    if (videofile)
	fclose(videofile);
    return 0;
}

struct option2 mainopts2[]= {
    {{"v", 1, NULL, OPT_VAL_IS_CHAR}, 0, "", 
     "Video to process"},
    {{"m", 1, NULL, OPT_VAL_IS_CHAR}, 0, "../data/model_test_31.xml",
     "The file containing the SMAT definition."},
    {{"format", 0, NULL, OPT_VAL_IS_CHAR}, 0, "", 
     "format = [raw | ra2 | avi]: Tells the format of the video"},
    {{"op", 0, NULL, true}, false, "", 
     "Saves the pose values after each frame. The file name will be <FILE>_pose_results.xml"},
    {{"ov", 0, NULL, true}, false, "", 
     "Saves a video of the results. They will be stored in video_results_SMAT.avi"
     " and video_results_FaceDir.avi"}, 
    {{"lines", 1, NULL, OPT_VAL_IS_INT}, 4, "", 
     "tIndicates de number of lines to draw which connects from each node"},
    {{"r", 0, NULL, true}, false, "", 
     "Read Model: Does not autocreate the Model. Reads if from a file"},
    {{"wait", 1, NULL, OPT_VAL_IS_INT}, 0, "", 
     "tIndicates de parameter for the cvWaitKey(n)"},
    {{"cam", 0, NULL, true}, false, "", 
     "Capture de video from the cameras"},

    {{"dat", 1, NULL, OPT_VAL_IS_INT}, 2, "", 
     "indica el numero N del archivo .daN a leer como datos del modelo"},
    {{"graficar", 0, NULL, true}, false, "", 
     "Muestra le proceso de matching 3D"},

    {{"options", 0, NULL, true}, false, "", 
     "Imprime las opciones de configuracion del programa"},
    {{"h", 0, NULL, true}, false, "", 
     ""},
    {{"inivect", 1, NULL, OPT_VAL_IS_CHAR}, 0, "",
     "Vector inicial de direcicón"},

    {{"iniframe", 1, NULL, OPT_VAL_IS_INT}, -1, "",
     "inidica el frame de inicio"},
    {{"h", 0, NULL, true}, false, "", 
     "Muestra las opciones que se pueden indicar por línea de comandos"},
    {{"help", 0, NULL, true}, false, "", 
     "Muestra esta ventana de ayuda"},
    {{NULL, 0, NULL, 0}, 0, "", 
     ""}
};
